perm filename THROW.SAI[LOU,BGB] blob
sn#081752 filedate 1974-12-08 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00010 PAGES
C REC PAGE DESCRIPTION
C00001 00001 VALID 00010 PAGES
C00005 00002 IFC BLUE THENC
C00006 00003 message procedure THROW(real array RELEASE_POINT, VELOCITY,
C00009 00004 simple procedure PREPARE_THR_POLY
C00012 00005 simple procedure CHECK_VELOCITY_BOUNDS
C00015 00006 simple procedure THR_POLY
C00020 00007 comment: Calculate polys for trajectory up to release point
C00025 00008 simple procedure THR_SCHEINMAN
C00030 00009 comment: this is the body of THROW
C00032 00010 simple procedure BALLIST(safe real array VEL, REL, TARGET real MUZ_VEL
C00034 ENDMK
C⊗;
IFC BLUE THENC
preload_with 4.5, 2.6, 1.0, 10.5, 7.5, 6.25;
ELSEC
preload_with 4.5, 2.6, .5, 10.5, 7.5, 6.25;
ENDC
safe real array MAX_VEL [1:6];
IFC BLUE THENC
preload_with 3.0, 3.7, .50, 27., 23., 7.4;
ELSEC
preload_with 3.0, 3.7, .29, 27., 23., 7.4;
ENDC
safe real array MAX_ACC [1:6];
define DEBUG = "false";
forward simple procedure UNSTRUCT(safe real array T, E);
message procedure THROW(real array RELEASE_POINT, VELOCITY,
FINAL_POINT; real FORE, AFT; reference boolean SUCCESS);
begin comment: Prepares trajectory for throwing motion, where
RELEASE_POINT[1:4,1:4] is the trans of the desired release point.
VELOCITY[1:4] is the X-Y-Z velocity (inches/jiffy),
FINAL_POINT[1:4,1:4] is the new rest position.
FORE is the proportion of velocity to be used for acceleration
at release minus.
AFT is the proportion of velocity to be used for acceleration
at release plus.
Returns SUCCESS = true ≡ everything went OK;
real safe own array JOINT_VELOCITY[1:6]; comment: Holds joint velocities
at time of release;
real safe own array ANGLES[1:7,1:6]; comment: Angle for [SEGMENT,JOINT];
integer safe own array SEG_TIME[1:7]; comment: Estimated time of major
segments;
integer JOINT;
safe real own array COE [1:30]; comment: Holds coefficients of polys;
safe real own array KOE [1:6,1:30]; comment: [joint,coef];
safe integer own array PERIOD [1:6,1:7]; comment: [joint,seg];
safe integer own array NO_SEGS [1:6]; comment: Number of segs for [joint];
simple procedure MODIFY_FIRST;; comment: Temporarily null;
simple procedure MODIFY_SECOND;; comment: Temporarily null;
integer REL_SEG; comment: the number of joint 6's release segment;
integer REL_TIME; comment: Time used for linear release segment;
simple procedure PREPARE_THR_POLY;
begin comment: Fills ANGLES with the joint angle for each
segment boundary;
real safe own array ANG[1:6]; comment: HOLDS A COLUMN OF ANGLES;
safe real own array TEMP[1:4]; safe real own array EULER[1:6];
safe real own array T[1:4,1:4];
integer I;
SUCCESS ← ¬ARM_SOLVE(LAST_TRANS,ANG);
ARRBLT(ANGLES[1,1],ANG[1],6);
UNIT(TEMP, DEPART_ARM);
UNSTRUCT(LAST_TRANS,EULER);
for I ← 1 step 1 until 3 do EULER[I] ← EULER[I] + TEMP[I];
CONSTRUCT(T, EULER);
SUCCESS ← ¬ARM_SOLVE(T,ANG);
if SUCCESS then ARRBLT(ANGLES[2,1],ANG[1],6) else return;
SUCCESS ← ¬ARM_SOLVE(RELEASE_POINT,ANG);
if SUCCESS then ARRBLT(ANGLES[4,1],ANG[1],6) else return;
SUCCESS ← ¬ARM_SOLVE(FINAL_POINT,ANG);
if SUCCESS then ARRBLT(ANGLES[7,1],ANG[1],6) else return;
UNIT(TEMP,ARRIVE_ARM);
UNSTRUCT(FINAL_POINT,EULER);
for I ← 1 step 1 until 3 do EULER[I] ← EULER[I] + ARRIVE_ARM[I];
CONSTRUCT(T,EULER);
SUCCESS ← ¬ARM_SOLVE(T,ANG);
if SUCCESS then ARRBLT(ANGLES[6,1],ANG[1],6);
return
end; comment: END OF PREPARE_THR_POLY;
simple procedure FILL_JOINT_VELOCITY;
begin comment: Computes the joint velocities at release point;
safe real own array TH [1:6]; comment: Angles at release point;
integer JOINT;
real TEMP1,TEMP2;
ARRBLT(TH[1],ANGLES[4,1],6);
HANDPOS(TH);
INCREMENT(JOINT_VELOCITY,VELOCITY,FALSE);
for JOINT ← 1 step 1 until 6 do
begin comment: compute angles at ends of linear release seg;
ANGLES[3,JOINT] ← (TEMP1 ← ANGLES[4,JOINT]) -
(TEMP2 ← JOINT_VELOCITY[JOINT] * REL_TIME/2.);
ANGLES[5,JOINT] ← TEMP1 + TEMP2;
end;
IFC DEBUG THENC
BEGIN
OUTSTR("JOINT VELOCITIES:"&CRLF);
FOR JOINT ← 1 STEP 1 UNTIL 6 DO
OUTSTR(CVG(JOINT_VELOCITY[JOINT]));
OUTSTR(CRLF);
END; ENDC
end;
simple procedure CHECK_VELOCITY_BOUNDS;
begin comment: Sets SUCCESS ← false if velocity at release point
is beyond any joint capacity;
integer I;
for I ← 1 step 1 until 3 do
SUCCESS ← SUCCESS ∧ (ABS(JOINT_VELOCITY[I]) ≤ MAX_VEL[I]);
ifc DEBUG thenc OUTSTR(CRLF&"VELOCITY BOUNDS YIELDS "&
CVG(SUCCESS)&CRLF); endc
end;
simple procedure COMPUTE_SEGMENT_TIMES;
begin comment: Estimates the time required for each segment;
integer SEGMENT, JOINT, TIME, T, TEMP;
for SEGMENT ← 1 ,7 do
begin
TIME ← 0;
for JOINT ← 1 step 1 until 6 do
begin
T ← ABS((ANGLES[(TEMP←if SEGMENT=1 then 1 else 6),JOINT]) - ANGLES[TEMP+1,JOINT])
* TIMFAC[JOINT];
if T > TIME THEN TIME ← T
end;
SEG_TIME[SEGMENT] ← TIME + 20
end;
for SEGMENT ← 2, 5 do
begin comment: medial segments;
TIME ← 0;
for JOINT ← 1 step 1 until 6 do
begin
T ← abs(ANGLES[SEGMENT+1,JOINT]-ANGLES[SEGMENT,JOINT]-
5.*JOINT_VELOCITY[JOINT]) * TIMFAC[JOINT] * 2.0;
if T > TIME then TIME ← T;
end;
SEG_TIME[SEGMENT] ← SEG_TIME[SEGMENT+1]← (TIME+20) % 2;
end;
SEG_TIME[4] ← REL_TIME;
ifc DEBUG thenc
OUTSTR(CRLF&"SEGMENT TIMES ARE:"&CRLF);
for segment ← 1 step 1 until 7 do
OUTSTR(CVG(SEG_TIME[SEGMENT]));
OUTSTR(CRLF); endc
end;
simple procedure THR_POLY;
begin comment: Generates the coefficients of the 5 polynomials
for JOINT, storing them in COE. If they do not meet restric-
tions, attempt is made to fix. If can't, SUCCESS ← false;
safe integer own array TAU [1:7]; comment: Time per segment;
safe real own array LU, A [1:9,1:9]; comment: Ax = b;
safe real own array B, X [1:9];
safe real own array Y[1:9]; comment: Holds data related to
ANGLES, JOINT_VELOCITY;
boolean OK;
real TEMP;
comment: The following are temporarily diagnostic;
simple procedure STAT3(integer SEG);
begin comment: Outputs statistics on the polynomial for SEG;
real A0, A1, A2, A3, TEMP;
A0 ← COE[TEMP ← 4*SEG -2];
A1 ← COE[TEMP + 1];
A2 ← COE[TEMP + 2];
A3 ← COE[TEMP + 3];
OUTSTR(CRLF&"STATISTICS FOR JOINT "&CVG(JOINT)&",SEGMENT "&
CVG(SEG));
OUTSTR(CRLF&"AT 0: "&CVG(A0)&CVG(A1/(TEMP←TAU[SEG]))&
CVG(2.*A2/(TEMP*TEMP)));
OUTSTR(CRLF&"AT 1: "&CVG(A0+A1+A2+A3)&CVG((A1+2.*A2+3.*A3)/TEMP)
&CVG((2.*A2+6.*A3)/(TEMP*TEMP)));
OUTSTR(CRLF&"MAX VEL IN SEG: "&CVG((A1-(A2*A2/(3.*A3)))/TEMP)&CRLF);
end;
simple procedure INSPECT_FIRST;
begin comment: Checks out initial part of trajectory;
real A0,A3,A4,TEMP;
OK ← true;
A0 ← COE[1];
A3 ← COE[4];
A4 ← COE[5];
OUTSTR(CRLF&"STATISTICS FOR LIFTOFF, JOINT "&CVG(JOINT));
OUTSTR(CRLF&"AT 0: "&CVG(A0)&CVG(0.)&CVG(0.));
OUTSTR(CRLF&"AT 1: "&CVG(A0+A3+A4)&CVG((3.*A3+4.*A4)/(TEMP←
TAU[1]))&CVG((6.*A3+12.*A4)/(TEMP*TEMP)));
OUTSTR(CRLF&"MAX VEL, ACC: "&CVG(A3↑3/(4.*A4*A4*TEMP))&
CVG(-.75*A3*A3/(A4*TEMP*TEMP))&CRLF);
STAT3(2);
STAT3(3);
end;
simple procedure INSPECT_SECOND;
begin comment: Checks out later part of trajectory;
real A0,A1,A2,A3,A4,TEMP, X,Y,Z;
OK ← true;
STAT3(4);
STAT3(5);
STAT3(6);
A0 ← COE[26];
A1 ← COE[27];
A2 ← COE[28];
A3 ← COE[29];
A4 ← COE[30];
OUTSTR(CRLF&"STATISTICS FOR SETDOWN, JOINT "&CVG(JOINT));
OUTSTR(CRLF&"AT 0: "&CVG(A0)&CVG(A1/(TEMP←TAU[7]))
&CVG(2.*A2/(TEMP*TEMP)));
OUTSTR(CRLF&"AT 1: "&CVG(A0+A1+A2+A3+A4)&CVG((A1+2.*A2+3.*A3+
4.*A4)/TEMP)&CVG((2.*A2+6.*A3+12.*A4)/(TEMP*TEMP)));
if (Y ← 9.*A3*A4-24.*A2*A4)>0 then
begin if (X←(-3.*A3+(Y←SQRT(Y)))/(12.*A4))<0
or X>1 then X ← X-Y/(12.*A4) end
else X ← 1;
OUTSTR(CRLF&"MAX VEL, ACC: "&CVG((A1+2.*A2*X+3.*A3*X*X+4*A4*X*X*X)
/TEMP)&CVG((2.*A2-.75*A3*A3/A4)/(TEMP*TEMP))&CRLF);
end;
comment: Initialize Y;
Y[1] ← ANGLES[1,JOINT];
Y[2] ← ANGLES[2,JOINT];
Y[3] ← ANGLES[3,JOINT];
Y[4] ← ANGLES[5,JOINT];
Y[5] ← ANGLES[6,JOINT];
Y[6] ← ANGLES[7,JOINT];
Y[7] ← JOINT_VELOCITY[JOINT];
Y[8] ← Y[7] * FORE;
Y[9] ← Y[7] * AFT;
comment: Initialize COE;
COE[1] ← Y[1];
COE[2] ← COE[3] ← COE[16] ← COE[17] ← 0.0;
COE[6] ← Y[2];
COE[18] ← Y[4];
COE[26] ← Y[5];
NO_SEGS[JOINT] ← 7;
ARRBLT(TAU[1],SEG_TIME[1],7);
comment: Calculate polys for trajectory up to release point;
OK ← false;
B[2] ← 0.0;
ARRBLT(B[3],B[2],4);
B[1] ← Y[2] - Y[1];
B[4] ← -Y[2];
B[7] ← Y[3];
B[8] ← Y[7];
B[9] ← Y[8] * (TAU[3]↑2)/2.;
while ¬OK do
begin
comment: Fill A;
A[1,1] ← 0;
ARRBLT(A[1,2],A[1,1],80);
A[1,1] ← A[1,2] ← A[4,3] ← A[4,4] ← A[4,5] ← A[7,6] ←
A[7,7] ← A[7,8] ← A[7,9] ← A[9,8] ← 1.0;
A[4,6] ← -1.0;
A[9,9] ← 3.0;
A[2,1] ← 3./(TEMP←TAU[1]);
A[2,2] ← 4./TEMP;
A[3,1] ← 3./(TEMP ← TEMP*TEMP);
A[3,2] ← 6./TEMP;
A[2,3] ← -(A[5,3] ← TEMP ← 1./TAU[2]);
A[5,5] ← (A[5,4] ← TEMP + TEMP) + TEMP;
A[3,4] ← -(A[6,4] ← (TEMP ← TEMP * TEMP));
A[6,5] ← 3. * TEMP;
A[5,7] ← -(TEMP ← A[8,7] ← 1./TAU[3]);
A[8,9] ← (A[8,8] ← TEMP + TEMP) + TEMP;
A[6,8] ← -TEMP * TEMP;
comment: Call the equation solver;
DECOMPOSE(9,A,LU);
SOLVE(9,LU,B,X);
comment: Save results in COE;
ARRBLT(COE[4],X[1],2);
ARRBLT(COE[7],X[3],7);
ifc DEBUG thenc INSPECT_FIRST; elsec OK ← true; endc
if ¬OK then MODIFY_FIRST
end;
if ¬SUCCESS then return;
comment: calculate poly for release segment;
COE[14] ← Y[3];
COE[15] ← REL_TIME * JOINT_VELOCITY[JOINT];
REL_SEG ← 4;
comment: Calculate polys for second half of trajectory;
OK ← false;
B[3] ← 0.;
ARRBLT(B[4],B[3],6);
B[1] ← Y[4] + (Y[9]*TAU[5]/2. + Y[7])*TAU[5];
B[2] ← Y[7] + Y[9] * TAU[5];
B[4] ← Y[5];
B[7] ← Y[6] - Y[5];
B[3] ← -Y[9]/2.;
while ¬OK do
begin
comment: Fill A;
A[1,1] ← 0.0;
ARRBLT(A[1,2],A[1,1],80);
A[1,2] ← A[4,2] ← A[4,3] ← A[4,4] ← A[4,5] ← A[7,6] ←
A[7,7] ← A[7,8] ← A[7,9] ← A[8,6] ← A[9,7] ← 1.0;
A[1,1] ← -1.;
A[8,7] ← 2.;
A[8,8] ← A[9,8] ← 3.;
A[8,9] ← 4.;
A[9,9] ← 6.;
A[2,1] ← -3. * (TEMP ← (1./TAU[5]));
A[3,1] ← 3. * TEMP * TEMP;
A[5,3] ← A[2,3] ← TEMP ← 1./TAU[6];
A[5,5] ← (A[5,4] ← TEMP + TEMP) + TEMP;
A[3,4] ← -(A[6,4] ← TEMP ← TEMP * TEMP);
A[6,5] ← 3. * TEMP;
A[6,7] ← (A[5,6] ← -(TEMP ← 1./TAU[7])) * TEMP;
comment: call the equation solver;
DECOMPOSE(9,A,LU);
SOLVE(9,LU,B,X);
comment: Transfer answers into COE;
COE[20] ← Y[9] * (TAU[5]↑2) / 2.;
ARRBLT(COE[21],X[1],5);
ARRBLT(COE[27],X[6],4);
COE[19] ← TAU[5] * Y[7];
ifc DEBUG thenc INSPECT_SECOND; elsec OK ← true; endc
if ¬OK then MODIFY_SECOND;
end;
comment: Store results in globals;
ARRBLT(KOE[JOINT,1],COE[1],30);
ARRBLT(PERIOD[JOINT,1],TAU[1],7);
return;
end;
simple procedure THR_SCHEINMAN;
comment: This drives SCHEINMAN for a throw;
begin
safe real own array DIAG [1:7]; comment: Holds results of SCHEINMAN;
safe real own array TH [1:6]; comment: Holds angles for SCHEINMAN;
integer SEG, JOINT, TEMP, I, P2SAVE, LIM, TIME, CUM;
real X;
TIME ← 0;
P2SAVE ← PTR2 - 7;
LIM ← NO_SEGS[6] - 1;
for SEG ← 1 step 1 until LIM do
begin comment: Treat middle segments;
TIME ← TIME + PERIOD[6,SEG];
for JOINT ← 1 step 1 until 6 do
begin
I ← CUM ← 0;
while CUM ≤ TIME do
begin
I ← I + 1;
CUM ← CUM + PERIOD[JOINT,I];
end;
comment: Now I is the correct segment. Must get X;
X ← 1. - (CUM - TIME) / PERIOD[JOINT,I];
if X = 0. then TH[JOINT] ← KOE[JOINT,4*I-2]
else TH[JOINT] ← (((KOE[JOINT,TEMP←4*I+1]*X)+
KOE[JOINT,TEMP-1])*X + KOE[JOINT,TEMP-2])*X
+ KOE[JOINT,TEMP-3];
end;
SCHEINMAN(DIAG,TH);
if SEG = REL_SEG then
DIAG[7] ← DIAG[7] LOR '4000000
else if SEG = REL_SEG + 1 then
OBJECT_MASS ← 0.
else DIAG[7] ← DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
end;
comment: Final segment;
for JOINT ← 1 step 1 until 6 do
TH[JOINT] ← KOE[JOINT,TEMP←NO_SEGS[JOINT]*4 - 2]
+ KOE[JOINT,TEMP+1] + KOE[JOINT,TEMP+2] + KOE[JOINT,TEMP+3]
+ KOE[JOINT,TEMP+4];
SCHEINMAN(DIAG,TH);
ARRBLT(COEFF[PTR2 - 7], DIAG[1],7);
PTR2 ← P2SAVE;
end;
simple procedure THR_PACK_UP;
begin comment: Drives PACK for throws, to assemble output buffer;
safe real own array BUF [0:4]; comment: Holds 5 coefficients;
safe own integer array NXT [1:6];
integer SEG;
COEFF[PTR4 ← PTR4 + 1] ← PERIOD[6,2];
STACK[PTR3 ← PTR3 + 1] ← '21000000 LOR PTR4;
for JOINT ← 1 step 1 until 6 do
begin comment: First segment;
ARRBLT(BUF[0], KOE[JOINT,1], 5);
PACK(COEFF[PTR4 ← PTR4 + 4],PERIOD[JOINT,1],BUF[0]);
NXT[JOINT] ← PTR4;
end;
COEFF[PTR4 ← PTR4 + 1] ← PTR2; comment: Joint 6 has extra word
pointing to Scheinman coefficients;
for JOINT ← 1 step 1 until 6 do
begin comment: Middle segments;
BUF[4] ← 0;
for SEG ← 2 step 1 until NO_SEGS[JOINT] - 1 do
begin
ARRBLT(BUF[0],KOE[JOINT,4*SEG - 2],4);
PACK(COEFF[PTR4 ← PTR4 + 4], PERIOD[JOINT,SEG],BUF[0]);
COEFF[NXT[JOINT]] ← (PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
NXT[JOINT] ← PTR4;
if JOINT = 6 then COEFF[PTR4 ← PTR4 + 1] ← PTR2 ← PTR2 - 7;
end;
comment: Final segment;
ARRBLT(BUF[0],KOE[JOINT,4*SEG - 2],5);
PACK(COEFF[PTR4 ← PTR4 + 4],PERIOD[JOINT,SEG],BUF[0]);
COEFF[NXT[JOINT]] ← (PTR4 lsh 18) lor COEFF[NXT[JOINT]];
if JOINT = 6 then COEFF[PTR4 ← PTR4 + 1] ← PTR2 ← PTR2 - 7;
end;
end;
comment: this is the body of THROW;
SUCCESS ← false;
if ARM_EXECUTE then return;
FLUSHP(300,LAST_ARM);
FRST_OPEN ← TRUE;
REL_TIME ← 12;
PREPARE_THR_POLY;
if ¬SUCCESS then return;
FILL_JOINT_VELOCITY;
ifc DEBUG thenc
begin integer I, J;
OUTSTR("END OF PREPARE_THR_POLY"&CRLF);
for I ← 1 step 1 until 7 do
begin
for j ← 1 step 1 until 6 do
OUTSTR(CVG(ANGLES[I,J]));
OUTSTR(CRLF);
end;
end; endc
CHECK_VELOCITY_BOUNDS;
if ¬SUCCESS then return;
COMPUTE_SEGMENT_TIMES;
for JOINT ← 1 step 1 until 6 do
begin
THR_POLY;
if ¬SUCCESS then return
end;
THR_SCHEINMAN;
THR_PACK_UP;
ARRBLT(LAST_TRANS[1,1],FINAL_POINT[1,1],16);
ARRBLT(LAST_ARM[1],ANGLES[5,1],6);
ARRBLT(LAST_PLANNED_TRANS[1,1],FINAL_POINT[1,1],16);
ARRBLT(LAST_PLANNED_ARM[1],ANGLES[5,1],6);
RESET_CONO;
FRST_OPEN ← false;
comment: Achieve hand opening;
STACK[PTR3 ← PTR3 + 1] ← '1000000 LOR (5. LSH -18);
end; comment: End of THROW;
simple procedure BALLIST(safe real array VEL, REL, TARGET; real MUZ_VEL;
reference boolean SUCCESS);
begin comment: Given REL [1:4,1:4], TARGET [1:3], and MUZ_VEL,
determines the x - y - z velocities to achieve the throw;
real DELX, DELY, DELZ, DELX2, DELY2, THETA, TRIAL, TEMP, C, S, CON, DERIV;
real DIST;
THETA ← 0.;
C ← 1.;
S ← 0.;
DELX2 ← (DELX ← TARGET[1] - REL[1,4]) * DELX;
DELY2 ← (DELY ← TARGET[2] - REL[2,4]) * DELY;
DIST ← SQRT(DELX2 + DELY2);
DELZ ← REL[3,4] - TARGET[3];
CON ← -4. * DIST↑2 / (MUZ_VEL↑2 * 75.);
TRIAL ← CON + DELZ;
SUCCESS ← true;
while abs(TRIAL) ≥ .00001 do
begin comment: Newton's method iteration;
DERIV ← CON * S /(C*C) + DIST*C - DELZ*S;
if abs(DERIV) ≤ .01 then
begin
SUCCESS ← false;
done
end;
THETA ← THETA - TRIAL/DERIV;
if abs(THETA) ≥ 3.0 then
begin
SUCCESS ← false;
done;
end;
TRIAL ← CON / (C ← COS(THETA)) +
DIST * (S ← SIN(THETA)) + DELZ*C;
end;
if SUCCESS then
begin
VEL[1] ← DELX * (TEMP ← C*MUZ_VEL/DIST);
VEL[2] ← DELY * TEMP;
VEL[3] ← S * MUZ_VEL;
VEL[4] ← 1.;
end;
end;